// 产生模拟的数据
#include "MoniProvider.h"

MoniProvider::MoniProvider()
{

}


/**
 * @brief MoniProvider::GenRobotPos
 * @param start---------相机中心点
 * @param len_Y
 * @param len_Z
 * @param target_Y 终点Y
 * @param target_Z 终点Z
 * @param arg 变位机参数
 * @param _index 数据库索引，默认为空
 * @param ror 饶TCP的旋转分量
 * @param isDB 标志位：是否数据库产生？
 * @return
 * @details 流程   相机中心点->Tcp点->YZ方向增加分量->绕TCP旋转分量->新的相机中心点
 */
GenPosVector MoniProvider::GenRobotPos(RobotPos start, int len_Y, int len_Z, int target_Y, int target_Z
                                       ,MachineArgs arg,DBIndex _index,RobotPos ror,bool isDB)
{
    GenPosVector vect;
    bool rrr = false;   /* 格点标志位，走蛇型 */
    for(int i = -(target_Y/len_Y);i <= (target_Y/len_Y) ;i++){
        if(rrr){
            for(int j = (target_Z/len_Z);j >= -(target_Z/len_Z) ;j--){
                RobotPos start_copy = start;
                genPos(i * len_Y,j * len_Z,start_copy,ror);
                std::get<2>(vect).push_back(std::make_tuple(start_copy,YZ(i * len_Y,j * len_Z)));
            }
        }else{
            for(int j = -(target_Z/len_Z);j <= (target_Z/len_Z) ;j++){
                // RobotPos start_copy = RobotTCP::CAMERA > start;  // 将原始pos转换为tcp坐标
                RobotPos start_copy = start;
                genPos(i * len_Y,j * len_Z,start_copy,ror);
                std::get<2>(vect).push_back(std::make_tuple(start_copy,YZ(i * len_Y,j * len_Z)));
            }
        }
        rrr = !rrr;
    }
    std::get<0>(vect) = arg;
    std::get<1>(vect) = start;
    std::get<3>(vect) = isDB;
    std::get<4>(vect) = _index;
    std::get<5>(vect) = ror;
    return vect;
}

GenPosVector MoniProvider::GenRobotPos(RobotPos start, int len_Y, int len_Z, int target_Y, int target_Z, RobotPos ror)
{
    GenPosVector vect;
    MachineArgs arg;
    DBIndex _index;
    std::get<0>(vect) = arg;
    std::get<1>(vect) = start;
    std::get<3>(vect) = false;
    std::get<4>(vect) = _index;
    std::get<5>(vect) = ror;

    bool rrr = false;   /* 格点标志位，走蛇型 */
    for(int i = -(target_Y/len_Y);i <= (target_Y/len_Y) ;i++){
        if(rrr){
            for(int j = (target_Z/len_Z);j >= -(target_Z/len_Z) ;j--){
                RobotPos start_copy = start;
                start_copy = genPos(i * len_Y,j * len_Z,start_copy,ror);
                std::get<2>(vect).push_back(std::make_tuple(start_copy,YZ(i * len_Y,j * len_Z)));
            }
        }else{
            for(int j = -(target_Z/len_Z);j <= (target_Z/len_Z) ;j++){
                RobotPos start_copy = start;
                start_copy = genPos(i * len_Y,j * len_Z,start_copy,ror);
                std::get<2>(vect).push_back(std::make_tuple(start_copy,YZ(i * len_Y,j * len_Z)));
            }
        }
        rrr = !rrr;
    }

    return vect;
}

RobotPos MoniProvider::genPos(int y, int z, RobotPos start_copy,RobotPos ror)
{
    start_copy = RobotTCP::CAMERA > start_copy;  // 将原始pos转换为tcp坐标
    RobotPos temp = RobotPos::instance();
    temp.y += y;
    temp.z += z;
    temp.a = ror.a;
    temp.b = ror.b;
    temp.c = ror.c;
    Matrix4d mb2b;
    pos2Matrix(temp,mb2b,zyx);
    Matrix2pos(mb2b,temp,zyz);
    start_copy = start_copy << temp;
    start_copy = start_copy > RobotTCP::CAMERA;
    return start_copy;
    //
}
